#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip6.h>
#include <netinet/icmp6.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <net/if.h>
#include <netinet/ether.h>
#include <ifaddrs.h>

void join_group(int fd)
{
	struct ifaddrs *ifap;
	if (getifaddrs(&ifap) < 0) {
		perror("getifaddrs");
		exit(0);
	}
	char hname[INET6_ADDRSTRLEN], org[INET6_ADDRSTRLEN];
	struct ipv6_mreq mq;
	int ret;
	for (struct ifaddrs *ifa = ifap; ifa; ifa = ifa->ifa_next) {
		if (ifa->ifa_addr == NULL) {
			continue;
		}
		int family = ifa->ifa_addr->sa_family;
		if (family != AF_INET6) {
			continue;
		}
		printf("**************************************\n");
		struct sockaddr_in6 *addr = (void *)ifa->ifa_addr;
		if (IN6_IS_ADDR_LOOPBACK(&addr->sin6_addr)) {
			continue;
		}
		mq.ipv6mr_interface = if_nametoindex(ifa->ifa_name);
		if ((ret = inet_pton(AF_INET6, "ff02::1:ff00:0", &mq.ipv6mr_multiaddr)) != 1) {
			if (ret == 0) {
				printf("does not contain a character string representing a valid network address in the specified address family\n");
			} else {
				perror("inet_pton");
			}
			continue;
		}
		mq.ipv6mr_multiaddr.s6_addr32[3] = addr->sin6_addr.s6_addr32[3] | htonl(0xFF000000);
		inet_ntop(AF_INET6, &mq.ipv6mr_multiaddr, hname, sizeof(hname));
		inet_ntop(AF_INET6, &addr->sin6_addr, org, sizeof(org));
		if (setsockopt(fd, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mq, sizeof(mq)) < 0) {
			printf("org=%s maddr=%s if=%s\n", org, hname, ifa->ifa_name);
			perror("setsockopt");
			continue;
		}

		inet_pton(AF_INET6, "ff02::1", &mq.ipv6mr_multiaddr);
		if (setsockopt(fd, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mq, sizeof(mq)) < 0) {
			printf("org=%s maddr=%s if=%s\n", org, hname, ifa->ifa_name);
			perror("setsockopt");
			continue;
		}

		inet_pton(AF_INET6, "ff02::2", &mq.ipv6mr_multiaddr);
		if (setsockopt(fd, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mq, sizeof(mq)) < 0) {
			printf("org=%s maddr=%s if=%s\n", org, hname, ifa->ifa_name);
			perror("setsockopt");
			continue;
		}

		printf("JOIN GROUP org=%s group=%s if=%s\n", org, hname, ifa->ifa_name);
	}
}

int create_socket()
{
	int fd = socket(AF_INET6, SOCK_RAW, IPPROTO_ICMPV6);
	struct icmp6_filter filters;
	ICMP6_FILTER_SETBLOCKALL(&filters);
	ICMP6_FILTER_SETPASS(ICMP6_ECHO_REQUEST, &filters);
	ICMP6_FILTER_SETPASS(ICMP6_ECHO_REPLY, &filters);
	ICMP6_FILTER_SETPASS(ND_NEIGHBOR_ADVERT, &filters);
	ICMP6_FILTER_SETPASS(ND_NEIGHBOR_SOLICIT, &filters);
	ICMP6_FILTER_SETPASS(ND_ROUTER_SOLICIT, &filters);
	ICMP6_FILTER_SETPASS(ND_ROUTER_ADVERT, &filters);
	if (setsockopt(fd, IPPROTO_ICMPV6, ICMP6_FILTER, &filters, sizeof(filters)) < 0) {
		perror("setsockopt filter");
		exit(0);
	}
	if (fd < 0) {
		perror("socket");
		exit(0);
	}

	join_group(fd);

	int on = 1;
	if (setsockopt(fd, IPPROTO_IPV6, IPV6_RECVRTHDR, &on, sizeof(on)) < 0) {
		perror("setsockopt RTH");
		exit(0);
	}
	setsockopt(fd, IPPROTO_IPV6, IPV6_RECVPKTINFO, &on, sizeof(on));
	setsockopt(fd, IPPROTO_IPV6, IPV6_RECVHOPLIMIT, &on, sizeof(on));
	setsockopt(fd, IPPROTO_IPV6, IPV6_RECVHOPOPTS, &on, sizeof(on));
	setsockopt(fd, IPPROTO_IPV6, IPV6_RECVDSTOPTS, &on, sizeof(on));
	setsockopt(fd, IPPROTO_IPV6, IPV6_RECVTCLASS, &on, sizeof(on));
	return fd;
}

void analysis_nd_neighbor_solicit(struct nd_neighbor_solicit *nbs, int len)
{
	int left = len;
	char hname[INET6_ADDRSTRLEN];
	inet_ntop(AF_INET6, &nbs->nd_ns_target, hname, sizeof(hname));
	printf("target:%s ", hname);
	left -= sizeof(struct nd_neighbor_solicit);
	struct nd_opt_hdr *opt = (void *)(nbs + 1);
	for (; left;) {
		if (opt->nd_opt_type == 1) {
			printf("Source Link addr: %s\n", ether_ntoa((void *)(opt + 1)));
		}
		left -= opt->nd_opt_len * 8;
	}
}

void analysis_nd_neighbor_advert(struct nd_neighbor_advert *advert, int len)
{
	int left = len;
	char hname[INET6_ADDRSTRLEN];
	inet_ntop(AF_INET6, &advert->nd_na_target, hname, sizeof(hname));
	printf("target:%s ", hname);
	left -= sizeof(struct nd_neighbor_advert);
	if (left == 0) {
		printf("no opt\n");
	}
	struct nd_opt_hdr *opt = (void *)(advert + 1);
	for (; left;) {
		if (opt->nd_opt_type == 2) {
			printf("Target Link addr: %s\n", ether_ntoa((void *)(opt + 1)));
		} else {
			printf("\n");
		}
		left -= opt->nd_opt_len * 8;
	}
}

void analysis_nd_router_advert(struct nd_router_advert *radv, int len)
{
	struct nd_opt_rds_hdr {
		uint8_t nd_opt_rds_type;
		uint8_t nd_opt_rds_len;
		uint16_t nd_opt_rds_reserved1;
		uint32_t nd_opt_rds_lifetime;
		struct in6_addr nd_opt_rds_addr;
	};

	int left = len;
	printf("Cur Hop Limit=%u, Router Lifetime=%u\n", radv->nd_ra_curhoplimit, radv->nd_ra_router_lifetime);
	printf("reachable time=%u, retransmit timer=%u\n", radv->nd_ra_reachable, radv->nd_ra_retransmit);
	left -= sizeof(struct nd_router_advert);
	if (left == 0) {
		printf("no opt\n");
	}
	char hname[INET6_ADDRSTRLEN];
	struct nd_opt_hdr *opt = (void *)(radv + 1);
	char *ptr = (void *)opt;
	for (; left;) {
		if (opt->nd_opt_type == 1) {
			printf("Source Link addr: %s\n", ether_ntoa((void *)(opt + 1)));
		} else if (opt->nd_opt_type == 5) {
			struct nd_opt_mtu *mtu = (void *)opt;
			printf("MTU=%u\n", mtu->nd_opt_mtu_mtu);
		} else if (opt->nd_opt_type == ND_OPT_PREFIX_INFORMATION) {
			struct nd_opt_prefix_info *prefix = (void *)opt;
			printf("valid_time=%u preferred_time=%u\n", prefix->nd_opt_pi_valid_time, prefix->nd_opt_pi_preferred_time);
			inet_ntop(AF_INET6, &prefix->nd_opt_pi_prefix, hname, sizeof(hname));
			printf("prefix=%s preefix_len=%d\n", hname, prefix->nd_opt_pi_prefix_len);
		} else if (opt->nd_opt_type == 25) {
			struct nd_opt_rds_hdr *rds = (void *)opt;
			inet_ntop(AF_INET6, &rds->nd_opt_rds_addr, hname, sizeof(hname));
			printf("lifetime=%u dns=%s\n", rds->nd_opt_rds_lifetime, hname);
		}
		left -= opt->nd_opt_len * 8;
		ptr += opt->nd_opt_len * 8;
		opt = (void *)ptr;
	}
}

void analysis_icmp(struct msghdr *msg, int len)
{
	struct cmsghdr *cmsg = NULL;
	struct in6_pktinfo *pkt;
	char hname[INET6_ADDRSTRLEN], ifname[IF_NAMESIZE];
	for (cmsg = CMSG_FIRSTHDR(msg); cmsg; cmsg = CMSG_NXTHDR(msg, cmsg)) {
		if (cmsg->cmsg_level != IPPROTO_IPV6) {
			continue;
		}

		switch (cmsg->cmsg_type) {
		case IPV6_RTHDR:
			printf("RTHDR\n");
			break;
		case IPV6_PKTINFO:
			pkt = (void *)CMSG_DATA(cmsg);
			inet_ntop(AF_INET6, &pkt->ipi6_addr, hname, sizeof(hname));
			printf("rcv_if=%s dst_addr=%s\n", if_indextoname(pkt->ipi6_ifindex, ifname), hname);
			break;
		case IPV6_HOPLIMIT:
			printf("HOPLIMIT\n");
			break;
		case IPV6_HOPOPTS:
			printf("IPV6_HOPOPTS\n");
			break;
		case IPV6_DSTOPTS:
			printf("IPV6_DSTOPTS\n");
			break;
		case IPV6_TCLASS:
			printf("IPV6_TCLASS\n");
			break;
		default:
			printf("unkonow type %d\n", cmsg->cmsg_type);
		}
	}
	struct nd_router_advert *adv = NULL;
	struct nd_router_solicit *soli = NULL;
	struct icmp6_hdr *icmp = msg->msg_iov[0].iov_base;
	switch (icmp->icmp6_type) {
	case ICMP6_ECHO_REQUEST:
	case ICMP6_ECHO_REPLY:
		printf("type=%d code=%d\n", icmp->icmp6_type, icmp->icmp6_code);
		break;
	case ND_ROUTER_ADVERT:
		printf("recv ND_ROUTER_ADVERT\n");
		adv = (void *)icmp;
		analysis_nd_router_advert(adv, len);
		break;
	case ND_ROUTER_SOLICIT:
		printf("recv ND_ROUTER_SOLICIT\n");
		break;
	case ND_NEIGHBOR_ADVERT:
		printf("recv ND_NEIGHBOR_ADVERT\n");
		analysis_nd_neighbor_advert((void *)icmp, len);
		break;
	case ND_NEIGHBOR_SOLICIT:
		printf("recv ND_NEIGHBOR_SOLICIT\n");
		analysis_nd_neighbor_solicit((void *)icmp, len);
		break;
	}
}

int main()
{
	char buf[1024], control[2048], peer[INET6_ADDRSTRLEN];
	struct iovec vec = { buf, sizeof(buf) };
	struct sockaddr_storage addr;
	union {
		char buf[2048];
		struct cmsghdr align;
	} u;
	struct msghdr msg = { &addr, sizeof(addr), &vec, 1, u.buf, sizeof(u.buf), 0 };

	int fd = create_socket();
	int len = 0;
	for (;;) {
		msg.msg_control = u.buf;
		msg.msg_controllen = sizeof(u.buf);
		if ((len = recvmsg(fd, &msg, 0)) < 0) {
			perror("recvmsg");
			exit(0);
		}
		getnameinfo((void *)&addr, sizeof(addr), peer, sizeof(peer), NULL, 0, NI_NUMERICHOST | NI_NUMERICSERV);
		printf("*****************************************************\n");
		printf("recv data len=%d from=%s\n", len, peer);
		analysis_icmp(&msg, len);
	}
	return 0;
}